//
// Created by Pulsar on 2020/5/16.
//

#include <rc_task/rcGyroTask.h>
#include <iostream>
#include <base/slog.hpp>
#include <libserv/libserv.h>
#include <string>
#include <cstring>
#include <ctime>
#include <vector>
#include <rc_globalVarable/rc_gyro.h>
namespace RC {
    namespace Task {
        int gyro_function(void *arg, std::vector<char> message) {
            if (message.size() < 11)return 0;
            char *temp_char = new char[message.size()];
            for (int i = 0; i < (int)message.size() - 1; i++) {
                temp_char[i] = message[i];
            }
            struct STime stcTime;
            struct SAcc stcAcc;
            struct SGyro stcGyro;
            struct SAngle stcAngle;
            struct SMag stcMag;
            struct SDStatus stcDStatus;
            struct SPress stcPress;
            struct SLonLat stcLonLat;
            struct SGPSV stcGPSV;
            system("clear");
            switch (message[1]) {
                case 0x50:
                    memcpy(&stcTime, &temp_char[2], 8);
            printf("Time:20%d-%d-%d %d:%d:%.3f\r\n", (short)stcTime.ucYear, (short)stcTime.ucMonth,
                   (short)stcTime.ucDay, (short)stcTime.ucHour, (short)stcTime.ucMinute, (float)stcTime.ucSecond + (float)stcTime.usMiliSecond / 1000);
                    break;
                case 0x51:
                    memcpy(&stcAcc, &temp_char[2], 8);
                    std::cout << "Acc:"
                              << " " << (float) stcAcc.a[0] / 32768 * 16
                              << " " << (float) stcAcc.a[1] / 32768 * 16
                              << " " << (float) stcAcc.a[2] / 32768 * 16 << "\n";

                    break;
                case 0x52:
                    memcpy(&stcGyro, &temp_char[2], 8);
                    std::cout << "Gyro:"
                              << " " << (float) stcGyro.w[0] / 32768 * 2000
                              << " " << (float) stcGyro.w[1] / 32768 * 2000
                              << " " << (float) stcGyro.w[2] / 32768 * 2000 << "\n";
                    break;
                case 0x53:
                    memcpy(&stcAngle, &temp_char[2], 8);
                    std::cout << "Angle:"
                              << " " << (float) stcAngle.Angle[0] / 32768 * 180
                              << " " << (float) stcAngle.Angle[1] / 32768 * 180
                              << " " << (float) stcAngle.Angle[2] / 32768 * 180 << "\n";
                    break;
                case 0x54:
                    memcpy(&stcMag, &temp_char[2], 8);
                    std::cout << "Mag:"
                              << " " << stcMag.h[0]
                              << " " << stcMag.h[1]
                              << " " << stcMag.h[2] << "\n";
                    break;
                case 0x55:
                    memcpy(&stcDStatus, &temp_char[2], 8);
                    std::cout << "DStatus:"
                              << " " << stcDStatus.sDStatus[0]
                              << " " << stcDStatus.sDStatus[1]
                              << " " << stcDStatus.sDStatus[2]
                              << " " << stcDStatus.sDStatus[3] << "\n";
                    break;
                case 0x56:
                    memcpy(&stcPress, &temp_char[2], 8);
                    std::cout << "Pressure:"
                              << " " << stcPress.lPressure
                              << " " << (float) stcPress.lAltitude / 100 << "\n";
                    break;
                case 0x57:
                    memcpy(&stcLonLat, &temp_char[2], 8);
//            printf("Longitude:%ldDeg%.5fm Lattitude:%ldDeg%.5fm\r\n", stcLonLat.lLon / 10000000, (double)(stcLonLat.lLon % 10000000) / 1e5, stcLonLat.lLat / 10000000, (double)(stcLonLat.lLat % 10000000) / 1e5);
                    break;
                case 0x58:
                    memcpy(&stcGPSV, &temp_char[2], 8);
//            printf("GPSHeight:%.1fm GPSYaw:%.1fDeg GPSV:%.3fkm/h\r\n\r\n", (float)stcGPSV.sGPSHeight / 10, (float)stcGPSV.sGPSYaw / 10, (float)stcGPSV.lGPSVelocity / 1000);
                    break;
            }
            delete temp_char;
            return 0;
        }
        bool check_function(std::vector<char> package) {
            return true;
        }
        void run_gyro_task(rc_GyroDevice  rcGyroDevice){
            using namespace libserv;
            slog::info << "陀螺仪任务启动中" << slog::endl;
            Serial serial(rcGyroDevice.path, rcGyroDevice.freq,rcGyroDevice.head);
            serial.bind_call(gyro_function);
            serial.bind_package_check(check_function);
            serial.run();
            slog::info << "陀螺仪任务启动完成" << slog::endl;
        }
    }
}
